﻿#include "eigenfuncset.h"

EigenFunc::EigenFunc()

{}

EigenFunc::~EigenFunc()
{}

#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif

// 欧拉角转换为3*3旋转矩阵
Eigen::Matrix3d EigenFunc::TransEuler2Matrix(float Za, float Ya, float Xa)
{
	double yaw = Za * M_PI / 180 ;
	double roll = Ya * M_PI / 180 ;
	double pitch = Xa * M_PI / 180 ;
	Eigen::Vector3d Euler(yaw, roll, pitch);
	Eigen::Matrix3d dRotationMatrix;

	dRotationMatrix = Eigen::AngleAxisd(Euler[0], Eigen::Vector3d::UnitZ()) *
		Eigen::AngleAxisd(Euler[1], Eigen::Vector3d::UnitY()) *
		Eigen::AngleAxisd(Euler[2], Eigen::Vector3d::UnitX());
	return dRotationMatrix;
}

// 欧拉角坐标转换为4维旋转矩阵
Eigen::Matrix4d EigenFunc::TransEulerPostion2Matrix(const Position& pos)
{
	Eigen::Matrix3d dRotationMatrix = TransEuler2Matrix(pos.a, pos.b, pos.c);
	Eigen::Matrix4d posMat = Eigen::Matrix4d::Identity();
	posMat.block(0, 0, 3, 3) = dRotationMatrix;
	posMat(0, 3) = pos.x;
	posMat(1, 3) = pos.y;
	posMat(2, 3) = pos.z;
	return posMat;
}

cv::Mat EigenFunc::MatrixToMat4d(Eigen::Matrix4f& matMatrix4f)
{
	cv::Mat out = cv::Mat::eye(4, 4, CV_64FC1);
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			out.at<double>(i, j) = matMatrix4f(i, j);//float转double
		}
	}
	return out;
}
//Matrix4d 转 opencv mat
cv::Mat EigenFunc::MatrixToMat4d(Eigen::Matrix4d& matMatrix4d)
{
	cv::Mat HomMat4d = cv::Mat::eye(4, 4, CV_64FC1);

	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			HomMat4d.at<double>(i, j) = matMatrix4d(i, j);//float转double
		}
	}

	return HomMat4d;
}

Eigen::Matrix4f EigenFunc::Mat4d2Matrix4f(cv::Mat matMatrix4d)
{
	Eigen::Matrix4f fMatrix = Eigen::Matrix4f::Identity();
	if (!matMatrix4d.empty()) {
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				fMatrix(i, j) = matMatrix4d.at<double>(i, j);//double转float
			}
		}
	}
	return fMatrix;
}

void HomMat3dToRT(const cv::Mat& HomMat3d, cv::Mat* RMatrix, cv::Mat* Tvec)
{
	assert(HomMat3d.rows == 4 && HomMat3d.cols == 4);
	//assert(RMatrix.depth == CV_64FC1 && Tvec.depth == CV_64FC1);//需要保持类型相同
	cv::Mat SubMat = HomMat3d(cv::Range(0, 3), cv::Range(0, 3));//获取区间索引
	SubMat.copyTo(*RMatrix);
	cv::Mat SubMat2 = HomMat3d(cv::Range(0, 3), cv::Range(3, 4));
	SubMat2.copyTo(*Tvec);
}

void EigenFunc::HomMat3dToRVT(const cv::Mat& HomMat3d, cv::Mat* Rvec, cv::Mat* Tvec)
{
	assert(HomMat3d.rows == 4 && HomMat3d.cols == 4);
	cv::Mat RMatrix;
	HomMat3dToRT(HomMat3d, &RMatrix, Tvec);
	Rodrigues(RMatrix, *Rvec);
}
void EigenFunc::lines(cv::InputOutputArray img, const std::vector<cv::Point2f>& start,
	const std::vector<cv::Point2f>& end, const cv::Scalar& color, int thickness, int lineType, int shift)
{
	for (int i = 0; i < start.size() && i < end.size(); i++) {
		line(img, start[i], end[i], color, thickness, lineType, shift);
	}
}

QByteArray EigenFunc::ImgEncodeToByte(const cv::Mat& Image, const std::string& ext)
{
	if (Image.empty()) {
		return	{};
	}
	//图片编码
	std::vector<uchar> vuBuf;
	cv::imencode(ext, Image, vuBuf);
	QByteArray qbaImage(reinterpret_cast<const char*>(vuBuf.data()), vuBuf.size());//https://www.jb51.cc/c/116955.html

	return qbaImage; // qbaImage2.toUtf8();
}
